import serial
import struct
import json
import time
import matplotlib.pyplot as plt
import sympy as sp
from sympy.solvers import solve
import numpy as np
from scipy.optimize import least_squares


def residuals(p, base_stations, distances):
    x, y = p
    res = []
    for (bx, by), d in zip(base_stations, distances):
        res.append(np.sqrt((x - bx) ** 2 + (y - by) ** 2) - d)
    return res


def locate_tag_numerical(base_stations, distances):
    initial_guess = [0, 0]
    result = least_squares(residuals, initial_guess, args=(base_stations, distances))
    if result.success:
        return [(result.x[0], result.x[1])]
    else:
        return []


base_location = [(0, 0), (1, 0), (1, 1)]


def parse_data_packet(packet_data):
    # 去除末尾两个字节
    packet_data = packet_data[:-2]

    # 解包，注意字节序为小端序
    unpacked_data = struct.unpack('<4B 10H 2B', packet_data)
    distances = [unpacked_data[5] / 100, unpacked_data[6] / 100, unpacked_data[7] / 100]
    loc = locate_tag_numerical(base_location, distances)

    # 将解析结果封装成字典
    result = {
        'tag_address': unpacked_data[3],
        'frame_num': unpacked_data[4],
        'distance1': unpacked_data[5],
        'distance2': unpacked_data[6],
        'distance3': unpacked_data[7],
        'accel_x': unpacked_data[8] / 100.0,
        'accel_y': unpacked_data[9] / 100.0,
        'accel_z': unpacked_data[10] / 100.0,
        'roll': unpacked_data[11] / 100.0,
        'pitch': unpacked_data[12] / 100.0,
        'yaw': unpacked_data[13] / 100.0,
        'position': loc
    }
    return result


def main():
    # 注释掉串口参数配置
    ser = serial.Serial('COM5', 115200)  # 替换为你的串口参数

    # 创建一个列表用于存储解析后的数据
    data_list = []

    # 打开一个文件用于保存数据
    with open('data.json', 'a') as f:
        buffer = bytearray()  # 缓冲区用于存储读取的数据
        try:
            plt.ion()  # 开启交互模式
            fig, ax = plt.subplots()
            line, = ax.plot([], [], 'ro-')  # 初始化线条
            ax.set_xlim(-2, 2)
            ax.set_ylim(-2, 2)
            ax.set_xlabel('X position')
            ax.set_ylabel('Y position')
            ax.set_title('Tag Position Trajectory')

            # 定义基站的颜色
            colors = ['blue', 'green', 'red']

            # 绘制基站位置
            for i, (x, y) in enumerate(base_location):
                ax.scatter(x, y, color=colors[i], s=100, label=f'Base {i + 1}')

            # 添加文本注释
            text_accel = ax.text(0.05, 0.95, '', transform=ax.transAxes, fontsize=12, verticalalignment='top')
            text_position = ax.text(0.05, 0.90, '', transform=ax.transAxes, fontsize=12, verticalalignment='top')

            # 显示图例
            ax.legend()

            # 这里可以模拟数据读取和绘图
            # 假设生成一些示例数据以供可视化
            for _ in range(50):
                # 模拟位置
                x, y = np.random.uniform(-2, 2), np.random.uniform(-2, 2)
                line.set_xdata(np.append(line.get_xdata(), x))
                line.set_ydata(np.append(line.get_ydata(), y))

                # 更新加速度和位置的文本注释
                text_accel.set_text(f'Accel: X={0:.2f}, Y={0:.2f}, Z={0:.2f}')  # 假设加速度为0
                text_position.set_text(f'Position: X={x:.2f}, Y={y:.2f}')

                fig.canvas.draw_idle()
                plt.pause(0.1)  # 短暂暂停以便于更新图形

        except KeyboardInterrupt:
            print("\n检测到键盘中断，正在退出...")
        finally:
            plt.ioff()  # 关闭交互模式
            plt.show()
            # ser.close()  # 注释掉串口连接关闭


if __name__ == '__main__':
    main()
